import numpy as np
import cv2
import math
cimport numpy as np
cimport cython


DTYPE = np.uint8
ctypedef np.uint8_t DTYPE_t
ctypedef np.float32_t DTYPE_f
ctypedef np.int DTYPE_i

cdef unsigned char absSub(unsigned char v1, unsigned char v2):
    return v1-v2 if v1>v2 else v2-v1

@cython.boundscheck(False)
@cython.wraparound(False)

# instruction:
# python3 setup.py build_ext --inplace
# python3 detector_python.py
# generate lidar observation fast

def front_seg(np.ndarray[DTYPE_t, ndim=2] in_image,np.ndarray[DTYPE_t, ndim=2] out_image):
    cdef int height, width, i, j
    height = in_image.shape[0]
    width = in_image.shape[1]

    for i in range(height):
        for j in range(width):
            g = in_image[i, j]
            if g > 0:
                out_image[i, j] = 255
                break

# convert lidar image to 0-360 scan data
def lidar_to_scan(np.ndarray[DTYPE_t, ndim=2] lidar_image,np.ndarray[DTYPE_f, ndim=1] lidar_scan,pos_x,pos_y):

    cdef int height, width, i, j
    height = lidar_image.shape[0]
    width = lidar_image.shape[1]

    for i in range(height):
        for j in range(width):
            g = lidar_image[i, j]
            if g > 0:
                # calculate the angle of the vector from the center to the pixel
                angle = math.atan2(i-pos_y,j-pos_x)
                # convert angle to 0-pi
                angle = angle if angle>=0 else 2*math.pi+angle
                # convert angle to 0-360
                angle = angle*180/math.pi
                # calculate the distance of the vector from the center to the pixel
                distance = math.sqrt((i-pos_y)*(i-pos_y)+(j-pos_x)*(j-pos_x))

                lidar_scan[int(angle)] = distance





